
#ifndef _ATTITUDE_ESTIMATE_H_
#define _ATTITUDE_ESTIMATE_H_

#include <sensor_common.h>

#define ATTITUDE_ESTIMATE_THREAD_STACK_SIZE 1024
#define ATTITUDE_ESTIMATE_THREAD_PRIORITY 6
#define ATTITUDE_ESTIMATE_THREAD_TICK 15

#define SENSOR_STORE_MAX 1 // 存储的(加速度12字节 + 角速度12字节 + 地磁12字节)的最多个数，多了会覆盖

/* 姿态融合算法选择，默认使用Mahony，因此宏定义为1，其它设置为0 */
#define ATTITUDE_ESTIMATE_USING_MAHONY 1
#define ATTITUDE_ESTIMATE_USING_EKF 0
#define ATTITUDE_ESTIMATE_USING_SEPERATE 0 /* 不进行融合，由加速度计算得到俯仰/滚转角，由磁力计计算得到偏航角 */

/* 欧拉角 */
struct attitude_euler
{
    float pitch; // 俯仰
    float roll;  // 滚转
    float yaw;   // 偏航
};

/* 欧拉角速度degree/s */
struct attitude_euler_angular
{
    float pitch_angular; // 俯仰
    float roll_angular;  // 滚转
    float yaw_angular;   // 偏航
};

/// @brief 姿态解算的类型：IMU、AHRS
enum attitude_estimate_kind
{
    IMU,
    AHRS
};

struct sensor_estimate_data
{
    sensor_data_t *acce;
    sensor_data_t *gyro;
    sensor_data_t *mag;
};

/// @brief 获得当前的姿态
/// @param attitude [out] 欧拉角
/// @param attitude_angular [out] 欧拉角速度
/// @return
int get_attitude(struct attitude_euler *attitude, struct attitude_euler_angular *attitude_angular);

/// @brief 开启姿态解算线程
/// @param
/// @return 开启成功返回0
int start_attitude_estimate();

/// @brief 传感器线程调用此函数记录/更新当前的：加速度、角速度、磁场大小
/// @param acce
/// @param gyro
/// @param mag
/// @return
int update_attitude_sensor(sensor_data_t *acce, sensor_data_t *gyro, sensor_data_t *mag);

#endif